/*
 * @Description: 
 * @FilePath: /LQ_TC26xB_LIBtasking/src/AppSw/Tricore/MY/API/MPU9250.h
 * @Author: Wei Zhou
 * @Github: https://github.com/zromyk
 * @Date: 1970-01-01 08:00:00
 * @LastEditors: Wei Zhou
 * @LastEditTime: 2020-05-22 19:46:09
 * @Copyright: Copyright © 2017 muyiro. All rights reserved.
 */ 

#ifndef _DEV_MPU9250_H
#define _DEV_MPU9250_H

#include <math.h>
#include "device_conf.h"

#include "dev_oled.h"

// MPU9250端口配置
#ifdef STM32F10X
#include "stm32f10x.h"

#define MPU9250_RCC_APBx                    RCC_APB2Periph_GPIOA
#define MPU9250_RCC_APBx_CMD(APBx, state)   RCC_APB2PeriphClockCmd(APBx, state)
#define MPU9250_GPIOx                       GPIOA
#define MPU9250_SCL_GPIO_Pin                GPIO_Pin_1      //定义SCL引脚  可任意更改为其他IO
#define MPU9250_SDA_GPIO_Pin   	            GPIO_Pin_2      //定义SDA引脚  可任意更改为其他IO

// gpio读写设置
#define MPU9250_READ_SDA            GPIO_ReadInputDataBit(MPU9250_GPIOx, MPU9250_SDA_GPIO_Pin)  // 获取SDA引脚电平
#define MPU9250_SDA_L()             GPIO_ResetBits(MPU9250_GPIOx, MPU9250_SDA_GPIO_Pin)         // SDA口输出低电平
#define MPU9250_SDA_H()             GPIO_SetBits(MPU9250_GPIOx, MPU9250_SDA_GPIO_Pin)           // SDA口输出高电平
#define MPU9250_SCL_L()             GPIO_ResetBits(MPU9250_GPIOx, MPU9250_SCL_GPIO_Pin)         //IO口输出低电平
#define MPU9250_SCL_H()             GPIO_SetBits(MPU9250_GPIOx, MPU9250_SCL_GPIO_Pin)           //IO口输出高电平
//#define MPU9250_SDA_OUT()           {GPIOA->CRL&=0xFFFFF0FF;GPIOA->CRL|=3<<8;}                  //输出方向
//#define MPU9250_SDA_IN()            {GPIOA->CRL&=0xFFFFF0FF;GPIOA->CRL|=4<<8;}                  //输入方向
#define MPU9250_SDA_OUT()           do {                                                    \
                                        GPIO_InitTypeDef GPIO_InitStructure;                \
                                        MPU9250_RCC_APBx_CMD(MPU9250_RCC_APBx, ENABLE);     \
                                        GPIO_InitStructure.GPIO_Pin = MPU9250_SDA_GPIO_Pin; \
                                        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;    \
                                        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;   \
                                        GPIO_Init(MPU9250_GPIOx, &GPIO_InitStructure);      \
                                    } while(0)
#define MPU9250_SDA_IN()            do {                                                    \
                                        GPIO_InitTypeDef GPIO_InitStructure;                \
                                        MPU9250_RCC_APBx_CMD(MPU9250_RCC_APBx, ENABLE);     \
                                        GPIO_InitStructure.GPIO_Pin = MPU9250_SDA_GPIO_Pin; \
                                        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;       \
                                        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;   \
                                        GPIO_Init(MPU9250_GPIOx, &GPIO_InitStructure);      \
                                    } while(0)

#endif

#define MPU_TIM_COUNT_CYCLE     (0.01) // 定时器中断以秒为单位的周期，如1ms中断，MPU_TIM_COUNT_CYCLE = 0.001
#define _abs(x)                 ((x) >= 0 ? (x) : -(x))

// 四元数数据结构体
typedef struct
{
    float kp;           /*比例增益*/
    float ki;           /*积分增益*/
    float exInt;
    float eyInt;
    float ezInt;        /*积分误差累计*/

    float q0;           /*四元数*/
    float q1;
    float q2;
    float q3;	
    float rMat[3][3];   /*旋转矩阵*/
} ElemNode;

typedef struct
{
    struct 
    {
        int16     x;
        int16     y;
        int16     z;
    } acc, gyro;
    
    struct 
    {
        float     x;
        float     y;
        float     z;
    } accRaw, gyroRaw;

    struct
    {
        float       angle;
        float       gyro;
    } pitch, yaw, roll;

    ElemNode elem;
} MPUNode;

extern MPUNode mpu;

void mpu9250_init(void);        // 初始化MPU9250
void MPU9250_GetInfo(void);     // 放在定时器中断中执行

#endif
